Fix: Enable colored pointcloud when frames arrive asynchronously - #3487
Fix: Enable colored pointcloud when frames arrive asynchronously#3487JINGERGER wants to merge 1 commit into
Conversation
- Cache most recent color frame for pointcloud texturing - Call map_to() on pointcloud filter when color frame arrives - Use cached color frame when not present in depth frameset - Fixes issue where colored pointcloud was not published without enable_sync This allows colored pointcloud generation without requiring enable_sync=true, maintaining full framerate while providing proper RGB texture mapping. Tested on RealSense D555 with librealsense v2.57.6
![aikido-pr-checks[bot]](https://avatars.githubusercontent.com/in/898896?s=60&v=4){kind=small}
| if (_filter && color_frame) | ||
| { | ||
| // Cast to rs2::pointcloud and call map_to | ||
| auto pc_filter = std::static_pointer_cast<rs2::pointcloud>(_filter); |
There was a problem hiding this comment.
std::static_pointer_cast to rs2::pointcloud followed by pc_filter->map_to(...) can dereference an object of the wrong runtime type. Verify the filter is actually a pointcloud (e.g., dynamic type check or store as correct type) before calling map_to.
Details
✨ AI Reasoning
A newly added MapTexture method casts the stored _filter (shared_ptrrs2::filter) to a rs2::pointcloud with std::static_pointer_cast and then calls map_to on the result. If _filter does not actually point to a rs2::pointcloud instance at runtime, the static cast yields an invalid object and calling map_to may dereference invalid memory and cause a segmentation fault. The code checks only that _filter is non-null and that color_frame is valid; there is no runtime type verification before calling map_to. This change introduced the unsafe cast and direct call, which did not exist before. The problematic code is at the line performing the static_pointer_cast and subsequent method call.
🔧 How do I fix it?
Add null checks before dereferencing pointers, validate array bounds before access, avoid using pointers after free/delete, don't write to string literals, and prefer smart pointers in modern C++.
Reply @AikidoSec feedback: [FEEDBACK] to get better review comments in the future.
Reply @AikidoSec ignore: [REASON] to ignore this issue.
More info
There was a problem hiding this comment.
@JINGERGER can you handle this comment please?
|
@JINGERGER thanks for the PR! We will review and update. |
There was a problem hiding this comment.
Pull request overview
This PR fixes an issue where colored pointclouds were not being published when depth and color frames arrive asynchronously (without enable_sync=true). The solution caches the most recent color frame and uses it for texture mapping when depth frames arrive without a corresponding color frame in the same frameset.
Changes:
- Added color frame caching mechanism to preserve the most recent color frame
- Implemented
MapTexture()method to map cached color frames to the pointcloud filter - Modified pointcloud publishing logic to use cached color frame when texture frame is not present in frameset
Reviewed changes
Copilot reviewed 4 out of 4 changed files in this pull request and generated 4 comments.
| File | Description |
|---|---|
| realsense2_camera/include/base_realsense_node.h | Added _cached_color_frame member variable to store the most recent color frame |
| realsense2_camera/include/pointcloud_filter.h | Added MapTexture() method declaration and updated Publish() signature to accept cached color frame |
| realsense2_camera/src/base_realsense_node.cpp | Implemented color frame caching in frame callback and passed cached frame to pointcloud publisher |
| realsense2_camera/src/pointcloud_filter.cpp | Implemented MapTexture() method and updated Publish() to use cached color frame as fallback |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| { | ||
| std::lock_guard<std::mutex> lock_guard(_mutex_publisher); | ||
| if ((!_pointcloud_publisher) || (!(_pointcloud_publisher->get_subscription_count()))) | ||
| if (!_pointcloud_publisher || !(_pointcloud_publisher->get_subscription_count())) |
There was a problem hiding this comment.
Unnecessary style change removing parentheses around the first condition check. This change reduces consistency with the second condition which still uses parentheses, and doesn't provide any functional improvement. Consider keeping the original style for consistency.
| if (!_pointcloud_publisher || !(_pointcloud_publisher->get_subscription_count())) | |
| if (!(_pointcloud_publisher) || !(_pointcloud_publisher->get_subscription_count())) |
| std::string texture_source_name = _filter->get_option_value_description(rs2_option::RS2_OPTION_STREAM_FILTER, static_cast<float>(texture_source_id)); | ||
|
|
||
| if (use_texture) | ||
| { |
There was a problem hiding this comment.
This line has been moved outside the conditional blocks where it's used (lines 135 and 146-148), but texture_source_id may not be initialized at line 114 if use_texture is false. This could lead to using an uninitialized variable. The variable declaration should remain inside the conditional blocks where texture_source_id is guaranteed to be set.
| std::string texture_source_name = _filter->get_option_value_description(rs2_option::RS2_OPTION_STREAM_FILTER, static_cast<float>(texture_source_id)); | |
| if (use_texture) | |
| { | |
| if (use_texture) | |
| { | |
| std::string texture_source_name = _filter->get_option_value_description( | |
| rs2_option::RS2_OPTION_STREAM_FILTER, | |
| static_cast<float>(texture_source_id)); |
| // Try using cached color frame if texture source is Color | ||
| if (texture_source_id == RS2_STREAM_COLOR && cached_color_frame) | ||
| { | ||
| warn_count = 0; |
There was a problem hiding this comment.
When a cached color frame is available, warn_count is reset but the code doesn't verify that the cached frame format is compatible with available_formats. The cached frame should be validated against available_formats before being accepted as a valid texture source, similar to how frames from the frameset are checked at line 121-122.
| // Try using cached color frame if texture source is Color | |
| if (texture_source_id == RS2_STREAM_COLOR && cached_color_frame) | |
| { | |
| warn_count = 0; | |
| // Try using cached color frame if texture source is Color and format is compatible | |
| if (texture_source_id == RS2_STREAM_COLOR && cached_color_frame) | |
| { | |
| auto cached_format = cached_color_frame.get_profile().format(); | |
| if (available_formats.find(cached_format) != available_formats.end()) | |
| { | |
| warn_count = 0; | |
| } | |
| else | |
| { | |
| warn_count++; | |
| ROS_WARN_STREAM_COND(warn_count == DISPLAY_WARN_NUMBER, | |
| "No compatible format for pointcloud chosen texture " << texture_source_name); | |
| return; | |
| } |
| { | ||
| texture_frame_holder = (*texture_frame_itr); | ||
| } | ||
| else if (cached_color_frame && cached_color_frame.is<rs2::video_frame>()) |
There was a problem hiding this comment.
The check for cached_color_frame.is<rs2::video_frame>() is redundant with the check at line 188. Consider removing the type check here since it's validated again before use, or consolidate the logic to avoid duplicate validation.
| else if (cached_color_frame && cached_color_frame.is<rs2::video_frame>()) | |
| else if (cached_color_frame) |
This allows colored pointcloud generation without requiring enable_sync=true, maintaining full framerate while providing proper RGB texture mapping.
Tested on RealSense D555 with librealsense v2.57.6